//////////////////////////////////////////////////////////////////////
//                                                                  //
//                                                                  //
//              Copyright (C), 2000  SEIKO EPSON Corp.              //
//              ALL RIGHTS RESERVED                                 //
//                                                                  //
//                                                                  //
//   file name : sqrt.c                                             //
//                                                                  //
//   Revision history                                               //
//       00/02/14    first release                 M.Igarashi       //
//                                                                  //
//////////////////////////////////////////////////////////////////////


#include <math.h>
#include <smcvals.h>

// ALGORITHM
// 1.devide x into xfrac and xexp
// 2.normalize |xfrac| < 2^6
// 3.calculate sqrt( x ) by Newton's Method 
//     xfrac = ( x/xfrac + xfrac ) / 2
//
 
//  the architecture of double floating point
//
//   0 1          11                  31 32                               63bit
//   -----------------------------------------------------------------------
//  | |   exponent  |                    fraction                           |
//   -----------------------------------------------------------------------
//  |               |                   |                                   |
//  |     12bits          20 bits       |             32 bits               |
//  |            lower word             |            higher word            |
//
//         bit    0         sign bit         (  1 bit  )
//              1 - 11      exponent part    ( 11 bits )
//             12 - 63      fraction part    ( 52 bits )
//
//

double sqrt(double dfX){

	long lX;
	unsigned long ulLx,ulHx;
	int	iXexp,iTemp;
	double	dfXfrac,dfRet,dfTemp;

	GETW_L(lX,dfX);		// get low
	GETW_H(ulHx,dfX);		// get high
	
	ulLx = lX&0x7fffffff;

	// error check
	if( (lX&0x80000000) !=0 ||( ulLx == 0x0 && ulHx == 0x0 )){
		if(ulLx != 0x0 || ulHx != 0x0){
			errno = EDOM;
			dfRet = NAN._D;
		}else{
			dfRet = 0.0;		// sqrt(0) =0
		}
		return dfRet;
	}
	dfXfrac = frexp(dfX,&iXexp);
	if((iXexp&0x1) !=0){
		dfXfrac *= 2.0;
		iXexp--;
	}
	
	// Newton iteration
	
	dfXfrac=0.5*(1.0+dfXfrac);     //(x+1.0)/2       initialize Newton iteration
	
	if(iXexp>=0){
		while(1){
			if(iXexp>50){               // normalize iXexp<50
				dfXfrac=dfXfrac*3.3554432e+7;     // xfrac*2^25  iXexp is 32 bit
				iXexp=iXexp-50;
			}else{
				break;
			}
		}
		iTemp=iXexp/2;
		dfTemp=1L<<iTemp;
		dfXfrac=dfXfrac*dfTemp;                 // normalize dfXfrac*2^6
	}else{ 
		while(1){
			if(iXexp<-50){              // normalize iXexp>-50
				dfXfrac=dfXfrac/3.3554432e+7;				//  xfrac/(2^25)  iXexp is 32 bit
				iXexp=iXexp+50;
			}else{
				break;
			}
		}
		iTemp=-iXexp/2;
		dfTemp=1L<<iTemp;
		dfXfrac=dfXfrac/dfTemp; 	// normalize dfXfrac*2^-6
	}
	
	for(iXexp=0;iXexp<=5;iXexp++){		// calc last Newton iteration
		dfTemp=dfX/dfXfrac;
		dfTemp+=dfXfrac;
		dfTemp*=0.5;
		dfXfrac=dfTemp;
	}
	
	dfRet = dfXfrac;
	return dfRet;
	
}
